/*========================================================================================================================================================================================================================
 * INCLUDES==============================================================================================================================================================================================================
 */

#include "HybridSystemController.h"

#include <LiquidCrystal.h>
#include <Servo.h>
#include <pt.h>
#include <ServoRate.h>

/*========================================================================================================================================================================================================================
 * DEFINES==============================================================================================================================================================================================================
 */

#define PIN_SERVO_THROTTLE 9
#define DEGREE_THROTTLE_LOW 60
#define RATE_THROTTLE_LOW 0
#define DEGREE_THROTTLE_HIGH 85
#define RATE_THROTTLE_HIGH 20
#define PIN_SERVO_CLUTCH 10
#define DEGREE_CLUTCH_DOWN 0
#define RATE_CLUTCH_DOWN 60
#define DEGREE_CLUTCH_MIDDLE 90
#define DEGREE_CLUTCH_UP 180
#define RATE_CLUTCH_UP 0
#define SECONDS_FULL_THROTTLE 5
#define LOAD_WHEEL_MIN_RPM 50
#define BUTTON_DEBOUNCE 100

#define SPEED_DRIVE_HIGH 255
#define ACC_DRIVE_UP_TO_HIGH 7843
#define SPEED_DRIVED_OFF 0
#define DEC_DRIVE_OFF 0
#define SPEED_DRIVE_LOW 20
#define ACC_DRIVE_UP_TO_LOW 100000

#define PIN_MOTOR_DRIVE 6

#define PIN_HYBRID_BATTERY A0

#define HYBRID_MODE_ACCELERATE 1
#define HYBRID_MODE_HOLD_SPEED 2
#define HYBRID_MODE_COAST 3

LiquidCrystal lcd( 12, 11, 5, 4, 3, 7 );

double getVoltage( struct TsVoltageMonitor & voltageMonitor );
double avgSensorReadings( struct TsVoltageMonitor & voltageMonitor );
void motorSetSpeed(
                    struct TsMotor & motor,
                    int nTargetSpeed,
                    unsigned long nAccRateInMicrosPerAcc );
int motorGetSpeed( struct TsMotor & motor );
int tachGetRpm( struct Tach &tach );

/*========================================================================================================================================================================================================================
 * VARIABLES==============================================================================================================================================================================================================
 */

int nHybridMode = 0;

struct TsMotor
{
  struct pt pt;
  struct TsMicroDelay tsMicroDelay;
  int nPin;
  int nCurrSpeed;
  int nTargetSpeed;
  unsigned long nAccRateInMicrosPerAcc;
  unsigned long nLastAccTimestampInMicros;
};

struct Tach
{
  unsigned long nMillisLast;
  unsigned long nMillisCurrent;
  unsigned int nTachDebounce;
};

struct TsVoltageMonitor
{
  struct pt pt;
  struct TsMicroDelay tsMicroDelay;
  struct pt ptStartEngine;
  struct pt ptStopEngine;
  struct pt threadKeepCharged;
  boolean bCheckedThisCoast;
  int nLowTripVolts;
  int nHighTripVolts;
  int nPin;
  int nSecondsBetweenVoltageChecks;
  int nPinMin;
  int nPinMax;
  int nVoltsMin;
  int nVoltsMax;
  int nSensorVal;
  double nVolts;
  double nVoltsTest1;
  double nVoltsTest2;
  double nVoltsTest3;
  double nVoltsTest4;
  double nVoltsTest5;
  double nVoltsTest6;
  double nVoltsTest7;
  double nVoltsTest8;
  double nVoltsTest9;
  double nVoltsTest10;
};

struct TsServo tsServoThrottle;
struct TsServo tsServoClutch;
struct TsMicroDelay tsMicroDelay;
struct TsMotor tsMotorDrive;
struct Tach tachLoadWheel;
struct TsVoltageMonitor tsVoltageMonitorBattery;

/*========================================================================================================================================================================================================================
 * PROTOTHREADS==============================================================================================================================================================================================================
 */
void motorInit( struct TsMotor &motor )
{
  PT_INIT( &motor.pt );
}

int motorGetSpeed( struct TsMotor & motor )
{
  return motor.nCurrSpeed;
}

void motorSetSpeed(
                    struct TsMotor & motor,
                    int nTargetSpeed,
                    unsigned long nAccRateInMicrosPerAcc )
{
  motor.nTargetSpeed = nTargetSpeed;
  motor.nAccRateInMicrosPerAcc = nAccRateInMicrosPerAcc;
}

#define __MOTOR_POS_NEXT(motor, nDiffFromTarget) (motor.nCurrSpeed) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
#define __MOTOR_DELAY_MICROS(motor) ( ( (motor.nAccRateInMicrosPerAcc) > 0 ) ? (motor.nAccRateInMicrosPerAcc) : 0 )
#define __MOTOR_ELAPSED_TIME(motor) ( micros() - motor.nAccRateInMicrosPerAcc )
#define __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME(servo) ( (__MOTOR_DELAY_MICROS(motor) > __MOTOR_ELAPSED_TIME(motor)) ? (__MOTOR_DELAY_MICROS(motor) - __MOTOR_ELAPSED_TIME(motor)) : 0  )
PT_THREAD( threadMotorController( struct TsMotor & motor ) )
{
  int nDiff = (int) motor.nTargetSpeed - (int) motor.nCurrSpeed;

  PT_BEGIN( &motor.pt )
    ;

    if ( nDiff != 0 )
    {
      analogWrite( motor.nPin, __MOTOR_POS_NEXT(motor, nDiff) );
      motor.nCurrSpeed = __MOTOR_POS_NEXT(motor, nDiff);
      if ( ( motor.nAccRateInMicrosPerAcc > 0 ) && ( motor.nTargetSpeed != motor.nCurrSpeed ) )
      {
        PT_SPAWN( &motor.pt, &motor.tsMicroDelay.pt, threadMicroDelay(motor.tsMicroDelay, __MOTOR_DELAY_MICROS_LESS_ELAPSED_TIME(TsMotor) ) );
      }
    }

  PT_END( &motor.pt );
return PT_ENDED;
}

PT_THREAD( threadDriveController( struct TsMotor & motor ) )
{
PT_BEGIN(&motor.pt)
  ;

// Step one: move clutch down until driveshaft begins to touch load wheel
  lcd.clear();
  lcd.print( "Step One" );
  servoSetPosition( tsServoClutch, DEGREE_CLUTCH_DOWN, RATE_CLUTCH_DOWN );
  PT_WAIT_UNTIL( &motor.pt, servoGetPosition(tsServoClutch) == DEGREE_CLUTCH_MIDDLE );

// Step two: begin to speed up motor while continuing to lower clutch
  lcd.clear();
  lcd.print( "Step Two" );
  motorSetSpeed( motor, SPEED_DRIVE_HIGH, ACC_DRIVE_UP_TO_HIGH );
  PT_WAIT_UNTIL( &motor.pt, motorGetSpeed( motor ) == SPEED_DRIVE_HIGH );

// Step three: wait for a bit
  nHybridMode = HYBRID_MODE_HOLD_SPEED;
  lcd.clear();
  lcd.print( "Step Three" );
  tsVoltageMonitorBattery.bCheckedThisCoast = false;
  PT_SPAWN( &motor.pt, &tsMicroDelay.pt, threadMicroDelay(tsMicroDelay, SECONDS_FULL_THROTTLE * 1000000) );

// Step four: raise clutch and turn off motor
  lcd.clear();
  lcd.print( "Step Four" );
  servoSetPosition( tsServoClutch, DEGREE_CLUTCH_UP, RATE_CLUTCH_UP );
  PT_WAIT_UNTIL( &motor.pt, servoGetPosition(tsServoClutch) == DEGREE_CLUTCH_MIDDLE );
  motorSetSpeed( motor, SPEED_DRIVED_OFF, DEC_DRIVE_OFF );

// Step five: wait until load wheel slows down
  nHybridMode = HYBRID_MODE_COAST;
  lcd.clear();
  lcd.print( "Step Five" );
  PT_WAIT_UNTIL( &motor.pt, tachGetRpm(tachLoadWheel) <= LOAD_WHEEL_MIN_RPM );

// Step six: raise motor speed to SPEED_DRIVE_LOW
  nHybridMode = HYBRID_MODE_ACCELERATE;
  motorSetSpeed( motor, SPEED_DRIVE_LOW, ACC_DRIVE_UP_TO_LOW );
  PT_WAIT_UNTIL( &motor.pt, motorGetSpeed( motor ) == SPEED_DRIVE_LOW );

PT_END(&motor.pt);
return PT_ENDED;
}

PT_THREAD( threadStartEngine( struct TsVoltageMonitor & voltageMonitor ) )
{
PT_BEGIN(&voltageMonitor.ptStartEngine)
;

//insert the engine starting sequence here

PT_END(&voltageMonitor.ptStartEngine);
return PT_ENDED;
}

PT_THREAD( threadStopEngine( struct TsVoltageMonitor & voltageMonitor ) )
{
PT_BEGIN(&voltageMonitor.ptStopEngine)
;

 //insert the engine stopping sequence here

PT_END(&voltageMonitor.ptStopEngine);
return PT_ENDED;
}

PT_THREAD( threadVoltageMonitor( struct TsVoltageMonitor & voltageMonitor ) )
{
PT_BEGIN(&voltageMonitor.pt)
;

if ( ( nHybridMode == HYBRID_MODE_COAST ) && ( voltageMonitor.bCheckedThisCoast == false ) )
{
voltageMonitor.nVoltsTest1 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest2 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest3 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest4 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest5 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest6 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest7 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest8 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest9 = getVoltage( voltageMonitor );
PT_SPAWN( &voltageMonitor.pt, &voltageMonitor.tsMicroDelay.pt, threadMicroDelay( voltageMonitor.tsMicroDelay, (voltageMonitor.nSecondsBetweenVoltageChecks * 1000000) ) );
voltageMonitor.nVoltsTest10 = getVoltage( voltageMonitor );
voltageMonitor.nVolts = avgSensorReadings( voltageMonitor );
voltageMonitor.bCheckedThisCoast = true;
}

PT_END(&voltageMonitor.pt);
return PT_ENDED;
}

PT_THREAD( threadKeepCharged( struct TsVoltageMonitor & voltageMonitor ) ) //Ask dad to check all my pt thingy stuff.
{
PT_BEGIN(&voltageMonitor.threadKeepCharged)
;

PT_SPAWN( &voltageMonitor.threadKeepCharged, &voltageMonitor.pt, threadVoltageMonitor( tsVoltageMonitorBattery ) );

if ( voltageMonitor.nVolts <= voltageMonitor.nLowTripVolts )
{
PT_SPAWN( &voltageMonitor.threadKeepCharged, &voltageMonitor.ptStartEngine, threadStartEngine( voltageMonitor ) );
}

if ( voltageMonitor.nVolts >= voltageMonitor.nHighTripVolts )
{
PT_SPAWN( &voltageMonitor.threadKeepCharged, &voltageMonitor.ptStopEngine, threadStopEngine( voltageMonitor ) );
}

PT_END(&voltageMonitor.threadKeepCharged);
return PT_ENDED;
}

/*========================================================================================================================================================================================================================
 * FUNCTIONS==============================================================================================================================================================================================================
 */

void tachInit( struct Tach &tach, int nMillisLastInit, int nMillisCurrentInit )
{
tach.nMillisLast = nMillisLastInit;
tach.nMillisCurrent = nMillisCurrentInit;
}

//void voltageMonitorInit( struct voltageMonitor &voltageMonitor,  )
void tachISR()
{
if ( ( millis() - tachLoadWheel.nMillisCurrent ) >= tachLoadWheel.nTachDebounce )
{
tachLoadWheel.nMillisLast = tachLoadWheel.nMillisCurrent;
tachLoadWheel.nMillisCurrent = millis();
}
}

int tachGetRpm( struct Tach &tach )
{
unsigned long nDiff = ( tach.nMillisCurrent - tach.nMillisLast );
return ( 60000 / nDiff );
}

double getVoltage( struct TsVoltageMonitor & voltageMonitor )
{
voltageMonitor.nSensorVal = analogRead( voltageMonitor.nPin );
return ( ( (double) ( voltageMonitor.nVoltsMax - voltageMonitor.nVoltsMin ) / (double) ( voltageMonitor.nPinMax - voltageMonitor.nPinMin ) ) * (double) voltageMonitor.nSensorVal );
}

double avgSensorReadings( struct TsVoltageMonitor & voltageMonitor )
{
return ( ( voltageMonitor.nVoltsTest1 + voltageMonitor.nVoltsTest2 + voltageMonitor.nVoltsTest3 + voltageMonitor.nVoltsTest4 + voltageMonitor.nVoltsTest5 + voltageMonitor.nVoltsTest6 + voltageMonitor.nVoltsTest7 + voltageMonitor.nVoltsTest8 + voltageMonitor.nVoltsTest9 + voltageMonitor.nVoltsTest10 ) / 10 );
}

/*========================================================================================================================================================================================================================
 * SETUP/LOOP==============================================================================================================================================================================================================
 */

//void setup()
//{
//lcd.begin( 16, 2 );
//lcd.print( "Initializing..." );
//
//motorInit( tsMotorDrive );
//
//attachInterrupt( 0, tachISR, FALLING );
//digitalWrite( 2, HIGH );
//tachInit( tachLoadWheel, 0, 0 );
//
//servoInit( tsServoThrottle, PIN_SERVO_THROTTLE, DELAY_SERVO_AIRTRONICS_94102, DEGREE_THROTTLE_LOW, 0 );
//}
//
//void loop()
//{
//threadServoController( tsServoThrottle );
//threadMotorController( tsMotorDrive );
//threadDriveController( tsMotorDrive );
//threadKeepCharged( tsVoltageMonitorBattery );
//}
